/*****************************************************
  Mars explorer NXT code in Ch
  University of Leicester GReP 2011
  Using http://iel.ucdavis.edu/projects/chmindstorms/
  Note: All distances are in cm
 *****************************************************/

#include <conio.h>
#include <stdio.h>
#include <ch_nxt.h>
#include <iostream.h>

#define MOVE_RATE 0.03 // = [actual distance moved in cm] / [motor rotation angle]
#define ROTATE_RATE 0.3333 // = [actual angle the robot rotates through] / [motor rotation angle]
#define TURRET_RATE 0.04 // = [actual angle the turret rotates through] / [motor rotation angle]

#define DEBUG

/*******************
  Global variables
 *******************/

struct nxt_remote nxtr;
char key;

/**************************
  Sensor class definitions
 **************************/

class Sensor
{
  private:
    int port;

  public:
    Sensor(int _port, int sensor, int mode); // setup sensor and checks for errors
    ~Sensor();

    int check();
};

Sensor::Sensor(int _port, int sensor, int mode)
{
  int status;

  port = _port;

  status = nxt_setsensor(port, sensor, mode);
  if (status==0)
  {
    cout << "\nError initializing sensor on port " << port << ".\nPress any key to exit." << endl;
    while (!_kbhit()); // wait for key press
    exit(0);
  }
}

Sensor::~Sensor() { }

int Sensor::check()
{
  int value;

  if (nxt_getsensor(port))
  {
    return nxtr.NXT_sensorvalraw[port];
  }
  else
  {
    cout << "Error checking sensor" << endl;
    return 0;
  }
}

/*********************
  Function prototypes
 *********************/

// Internal
double get_tacho(int port); // checks reading of motor tachometer
void seek_motor_pos(int port, double goal); // tweaks the motors to reach the target position

// External
void exitProg(); // Disconnects from NXT and exits program
void checkKey(); // checks if user has pressed a key and stores it, if q is entered quits the program
void setup(); // connects to the NXT and checks for errors
int disconnect(); // stop interfacing, this also stops the motors and sensors.

void moveCont(int speed); // sets the robot motors to move continuously
void moveStop(); // shuts off the robot movement motors
void moveStep(double distance, int speed); // moves the robot a given distance

void rotateCont(int speed); // turns the robot continuously
void rotateStep(int robotangle, int speed); // turns the robot by a given angle

void scanUltra(Sensor &ultra, Sensor &compass, int numpoints, double angle[], double distance[]);
void testTurret();

/**********************
  Function definitions
 **********************/

double get_tacho(int port)
{
  int i;
  
  for(i=0; i<3; i++)
  {
    if (nxt_gettacho(port)==1)
      return nxtr.NXT_motor_pos_cum[port];
    else
      cout << "Error: can't retrieve tachometer data, retrying" << endl;
  }
  
  cout << "Error: Can't retrieve tachometer data, giving up" << endl;
  return 0;
}

void seek_motor_pos(int port, double goal)
{
  int success, i, speed;

  speed = 10;
  for (i=0; i<2; i++)
  {  
    if (get_tacho(port) < goal) // when the motor needs to rotate forwards
    {
      nxt_motor(port, speed, RUN_BRAKE); // start the motor running continuously
      while (get_tacho(port) < goal) // wait for the motor to reach the goal
        checkKey();
      nxt_motor(port, 0, RUN_BRAKE); // stop the motor
    }
    else if (get_tacho(port) > goal) // when the motor needs to rotate backwards
    {
      nxt_motor(port, -speed, RUN_BRAKE); // start the motor running continuously
      while (get_tacho(port) > goal) // wait for the motor to reach the goal
        checkKey();
      nxt_motor(port, 0, RUN_BRAKE); // stop the motor
    }

    speed = 5;
  }
}

void exitProg()
{
  cout << "\nExiting program" << endl;
  disconnect();
  exit(0);
}

void checkKey()
{ 
  if (kbhit()) // kbhit() returns zero until a character has been entered
  {
    key = getch(); // store keyboard input
  }
  
  // Pressing q exits the loop
  if (key == 'q')
  {
    cout << "\nq key pressed" << endl;
    exitProg();
  }
}

void setup()
{
  // Connect to NXT, if connection fails wait for key press to quit.
  if (nxt_connect(&nxtr) == 0)
  {
    cout << "\nPress any key to quit.";
    while (!_kbhit()); // wait for key press
    exit(0);
  }
}

int disconnect()
{
  int a;

  a = nxt_disconnect();
  cout << endl;

  return a;
}

void moveCont(int speed)
{
  speed = -speed; // motors are currently backwards

  nxt_motor(PORT_B, speed, RUN_IDLE);
  nxt_motor(PORT_C, speed, RUN_IDLE);
}

void moveStop()
{
  nxt_motor(PORT_B, 0, OFF_IDLE);
  nxt_motor(PORT_C, 0, OFF_IDLE);
}

void moveStep(double distance, int speed)
{
  int angle;
  int arrived1, arrived2;
  int port1 = PORT_B;
  int port2 = PORT_C;
  double start1, start2;
  double goal1, goal2;

  checkKey();

  #ifdef DEBUG
    cout << "moveStep() distance(cm) " << distance << endl;
  #endif

  if (distance == 0) // don't move if given 0 distance
    return;

  angle = (int)(distance / MOVE_RATE); // get actual angle to rotate the motors through

  // Motors are currently reversed
  angle = -angle;
  speed = -speed;

  if (distance < 0) // rotate motors backwards if distance is negative
    speed = -speed;

  // Get initial tachometer positions
  start1 = get_tacho(port1);
  start2 = get_tacho(port2);

  // Set the desired motor positions
  goal1 = start1 + angle;
  goal2 = start2 + angle;

  // Start motors running
  nxt_motor(port1, speed, RUN_BRAKE);
  nxt_motor(port2, speed, RUN_BRAKE);
  
  // Run motors until they rotate past the desired position
  if (angle > 0)
  {
    while (!(arrived1 && arrived2)) // wait for both motors to reach the goal positions
    {
      checkKey();

      if (get_tacho(port1) > goal1)
      {
        nxt_motor(port1, 0, RUN_BRAKE);
        arrived1 = 1;
      }
      if (get_tacho(port2) > goal2)
      {
        nxt_motor(port2, 0, RUN_BRAKE);
        arrived2 = 1;
      }
    }
  }
  else if (angle < 0)
  {
    while (!(arrived1 && arrived2))
    {
      checkKey();

      if (get_tacho(port1) < goal1)
      {
        nxt_motor(port1, 0, RUN_BRAKE);
        arrived1 = 1;
      }
      if (get_tacho(port2) < goal2)
      {
        nxt_motor(port2, 0, RUN_BRAKE);
        arrived2 = 1;
      }
    }
  }

  // Adjust motor positions finely for accuracy
  seek_motor_pos(port1, goal1);
  seek_motor_pos(port2, goal2);

  // Shut off both motors to reduce power usage from braking
  nxt_motor(port1, 0, OFF_IDLE);
  nxt_motor(port2, 0, OFF_IDLE);
}

void rotateCont(int speed)
{
  checkKey();

  speed = -speed; // motors are currently backwards

  nxt_motor(PORT_B,  speed, RUN_IDLE);
  nxt_motor(PORT_C, -speed, RUN_IDLE);
}

void rotateStep(int robotangle, int speed)
{
  int angle;
  int arrived1=0, arrived2=0;
  int port1 = PORT_B;
  int port2 = PORT_C;
  int start1, start2;
  int goal1, goal2;

  checkKey();

  #ifdef DEBUG
    cout << "rotateStep() angle " << robotangle;
  #endif  
  
  if (robotangle == 0)
    return;

  angle = (int)(robotangle / ROTATE_RATE); // get actual angle to rotate the motors through
  
  // Motors are currently reversed
  angle = -angle;
  speed = -speed;

  if (robotangle < 0) // rotate motors backwards if angle is negative
  {
    #ifdef DEBUG
      cout << " rotate anti-clockwise" << endl;
    #endif
    speed = -speed;
  }
  else
  {
    #ifdef DEBUG
      cout << " rotate clockwise" << endl;
    #endif
  }

  // Get initial tachometer positions
  start1 = get_tacho(port1);
  start2 = get_tacho(port2);

  // Set the desired motor positions
  goal1 = start1 + angle;
  goal2 = start2 - angle;

  // Start motors running
  nxt_motor(port1, speed, RUN_BRAKE);
  nxt_motor(port2, -speed, RUN_BRAKE);

  // Run motors until they rotate past the desired position
  if (angle > 0) // if angle is POSITIVE
  {
    while (!(arrived1 && arrived2))
    {
      checkKey();

      if (get_tacho(port1) > goal1) // motor 1 reading must be LARGER
      {
        nxt_motor(port1, 0, RUN_BRAKE);
        arrived1 = 1;
      }
      if (get_tacho(port2) < goal2) // motor 2 reading must be SMALLER
      {
        nxt_motor(port2, 0, RUN_BRAKE);
        arrived2 = 1;
      }
    }
  }
  else if (angle < 0) // if angle is NEGATIVE
  {
    while (!(arrived1 && arrived2))
    {
      checkKey();
      
      if (get_tacho(port1) < goal1) // motor 1 reading must be SMALLER
      {
        nxt_motor(port1, 0, RUN_BRAKE);
        arrived1 = 1;
      }
      if (get_tacho(port2) > goal2) // motor 2 reading must be LARGER
      {
        nxt_motor(port2, 0, RUN_BRAKE);
        arrived2 = 1;
      }
    }
  }
  
  // Adjust motor positions finely for accuracy
  seek_motor_pos(port1, goal1);
  seek_motor_pos(port2, goal2);

  // Shut off both motors to reduce power usage from braking
  nxt_motor(port1, 0, OFF_IDLE);
  nxt_motor(port2, 0, OFF_IDLE);
}

void scanUltra(Sensor &ultra, Sensor &compass, int numpoints, double angle[], double distance[])
{
  int i;
  int speed = 25;
  int start;
  int current;
  int goal;
  int port = PORT_A;
  int stepsize = (360 / numpoints) / TURRET_RATE; // distance for motor to move between readings

  for (i=0; i<numpoints; i++)
  {
    angle[i] = 0;
    distance[i] = 512;
  }
  
  start = get_tacho(port); // store starting position of motor
  current = start;
  goal = start + (360 / TURRET_RATE);
  
  nxt_motor(port, speed, RUN_BRAKE); // start motor running

  i=0;
  while (current < goal) // loop until turret rotates 360 degrees
  {
    checkKey();

    current = get_tacho(port);
    
    if (current > (start + (i+1) * stepsize)) // wait until motor reaches next step position
    {
      //angle[i] = 360 - ((i+1) * stepsize * TURRET_RATE); // 360 - because it's rotating backwards
      angle[i] = 2 * compass.check(); // for magnetometer
      distance[i] = ultra.check();
      i++;
    }
  }
  nxt_motor(port, 0, RUN_BRAKE);

  // Return to start position
  nxt_motor(port, -60, RUN_BRAKE); // start motor running
  while (current > start) // wait for motor to return to start position
  {
    current = get_tacho(port);
    checkKey();
  }
  nxt_motor(port, 0, RUN_BRAKE); // stop motor

  // Fine position motor
  nxt_motor(port, 10, RUN_BRAKE); // rotate motor slowly for correction
  while (get_tacho(port) < start)
    checkKey();
  nxt_motor(port, 0, RUN_BRAKE); // stop motor
  
  nxt_motor(port, 0, OFF_IDLE); // shut off motor
}

void testTurret()
{
  int speed = 40;
  int start;
  int port = PORT_A;
  int stepsize = 90 / TURRET_RATE;

  start = get_tacho(port); // store starting position of motor

  nxt_motor(port, speed, RUN_BRAKE); // start motor running

  while (get_tacho(port) < (start + stepsize)) // loop until turret rotates 90 degrees
    checkKey();
  nxt_motor(port, 0, RUN_BRAKE);

  // Return to start position
  nxt_motor(port, -speed, RUN_BRAKE); // start motor running
  while (get_tacho(port) > start) // wait for motor to return to start position
    checkKey();
  nxt_motor(port, 0, RUN_BRAKE); // stop motor

  // Fine position motor
  nxt_motor(port, 10, RUN_BRAKE); // rotate motor slowly for correction
  while (get_tacho(port) < start)
    checkKey();
  nxt_motor(port, 0, RUN_BRAKE); // stop motor
  
  nxt_motor(port, 0, OFF_IDLE); // shut off motor
}